#include "ir/ir_sensor.h"

#include <iostream>
#include <fstream>
#include <ros/package.h>
#include <simulation/Distance.h>
#include <sstream>
#include <vector>

IRSensor::IRSensor(int id, float x, float y, float z, float angle, IRSensorType type) :
	id_(id),
	x_(x),
	y_(y),
	z_(z),
	angle_(angle),
	type_(type),
	current_average_position_(0)
{
	ros::NodeHandle nh("ir");

	nh.getParam("num_average_readings", num_average_readings_);

	average_readings_ = (int *) malloc(sizeof(int) * num_average_readings_);

	std::string path = ros::package::getPath("sensor");
	std::stringstream ss;
	ss << "/calibration" << id_;
	path += ss.str();
	readCalibrationFromFile(path);
}

void IRSensor::readCalibrationFromFile(std::string filename)
{
	ROS_INFO("Reading calibration file from %s", filename.c_str());

	std::ifstream file;
	file.open(filename.c_str(), std::ifstream::in);
	if (!file.good()) {
		ROS_ERROR("Could not open file at path %s", filename.c_str());
		return;
	}

	std::vector<float> dists;
	std::vector<int> adc_vals;

	while (!file.eof()) {
		float dist;
		int adc_val;

		file >> dist;
		file >> adc_val;

		dists.push_back(dist);
		adc_vals.push_back(adc_val);

		if (adc_vals.back() < 0 || adc_vals.back() > IR_SENSOR_ADC_STEPS) {
			ROS_ERROR("ADC value in input file out of range, should be in range [0, %d) is %d", IR_SENSOR_ADC_STEPS, adc_vals.back());
		}
	}

	min_range_ = dists[0];
	max_range_ = dists.back();

	// Initialize lookup table to 0
	for (int i = 0; i < IR_SENSOR_ADC_STEPS; i++) {
		distance_lookup_[i] = 0.0f;
	}

	// Set the first values to minimum measure value
	for (int i = adc_vals[0]; i < IR_SENSOR_ADC_STEPS; i++) {
		distance_lookup_[i] = min_range_;
	}

	for (unsigned int k = 0; k < adc_vals.size() - 2; k++) {
		int current_adc = adc_vals[k];
		int next_adc = adc_vals[k + 1];
		float current_distance = dists[k];
		float next_distance = dists[k + 1];

		int adc_length = std::abs(current_adc - next_adc);
		float dist_diff = next_distance - current_distance;
		float dist_per_step = dist_diff / adc_length;
		float prev = current_distance;
		for (int i = current_adc; i >= next_adc; i--) {
			distance_lookup_[i] = prev;
			prev += dist_per_step;
		}
	}

	for (int i = 0; i < IR_SENSOR_ADC_STEPS; i += 10) {
		ROS_DEBUG("distance: [%d]: %f\n", i, distance_lookup_[i]);
	}
}

float IRSensor::simulatedMeasure(ros::ServiceClient *distance_client)
{
	simulation::Distance srv;

	srv.request.position_x = x_;
	srv.request.position_y = y_;
	srv.request.direction = angle_;
	srv.request.min_range = min_range_;
	srv.request.max_range = max_range_;
	srv.request.mean = 0;
	srv.request.std_dev = 0;

	float measuredDistance = 0.0f;
	if (distance_client->call(srv)) {
		measuredDistance = srv.response.distance;
	} else {
		ROS_ERROR("Failed to call distance service.");
	}

	return measuredDistance;
}

float IRSensor::measure()
{
	int adc_average = 0;
	for (int i = 0; i < num_average_readings_; i++) {
		adc_average += average_readings_[i];
	}
	adc_average /= num_average_readings_;

	if (adc_average < 0 || adc_average >= IR_SENSOR_ADC_STEPS) {
		return 0.0f;
	}

	return distance_lookup_[adc_average];
}

void IRSensor::setADCValue(int value)
{
	current_average_position_ = (current_average_position_ + 1) % num_average_readings_;
	average_readings_[current_average_position_] = value;
}
